#include "remote.h"
#include "fly.h"
#include "bluetooth.h"
#include "usart.h"
#include "delay.h"
#include "beep.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

void RemoteControlTask(void* pram)
{
    static u8 BluetoothFlagRight = 0;
    static u8 BluetoothFlagLeft = 0;
    static float BluetoothFireThrust = 0.f;
    static u8 StopFlag = 0;
    while(1)
    {
        switch(GetBluetoothRight())
        {
            case 1:FlyAdvance();BluetoothFlagRight = 1;break;
            case 3:FlyRight();BluetoothFlagRight = 3;break;
            case 5:FlyBack();BluetoothFlagRight = 5;break;
            case 7:FlyRight();BluetoothFlagRight = 7;break;
            case 9:FlyStop();BluetoothFlagRight = 9;break;
            default:KeeplastFlagRight(BluetoothFlagRight);break;
        }
        switch(GetBluetoothLeft())
        {
            case 3:SetRotate(0);BluetoothFlagLeft = 3;break;
            case 7:SetRotate(1);BluetoothFlagLeft = 7;break;
            case 9:FlyStop();BluetoothFlagLeft = 9;break;
            default:KeeplastFlagLeft(BluetoothFlagLeft);break;
        }
        
        BluetoothFireThrust = GetBluetoothFire()*3.f;
        if(BluetoothFireThrust != 0)
        {
            SetFlyUp(BluetoothFireThrust);
        }
        if(GetBluetoothStop() == 1)
        {
            StopFlag = !StopFlag;
        }
        if(StopFlag ==1)
        {
            FlyStop();
            BEEP_Warning();
            printf("12345\r\n");
        }
        else
        {
            printf("54321\r\n");
        }
        vTaskDelay(100);
    }
}

void KeeplastFlagRight(u8 flag)
{
    if(flag==1)
    {
        FlyAdvance();
    }
    else if(flag==3)
    {
        FlyRight();
    }
    else if(flag==5)
    {
        FlyBack();
    }
    else if(flag==7)
    {
        FlyRight();
    }
    else if(flag==9)
    {
        FlyStop();
    }
}

void KeeplastFlagLeft(u8 flag)
{
    if(flag==3)
    {
        SetRotate(0);
    }
    else if(flag==7)
    {
        SetRotate(1);
    }
    else if(flag==9)
    {
        FlyStop();
    }
}

